#! /usr/bin/env python
# -*- coding:UTF-8 -*-
# 上面这两行代码必须要加上，否则在ros当中无法直接运行
# import rospy

import cv2
import numpy as np
import sys

# 11.30
# 这份代码，仅仅是三个圆形标牌的识别触发器，其功能是看到圆形的三个交通标志的时候，能够将标牌识别出来，并不能做区分


# 声明全局变量
global cap_camera_wig   # 用于进行视频帧捕获的一个cap变量（nano上用不到）

global blue_lower_rls   # 用于判定rls蓝色掩膜的HSV蓝色空间的上下限
global blue_upper_rls

global kernel_set   # 用于进行形态学开操作的卷积核心

global blue_lower_pau_turn  # 用于判定Pau和Turn蓝色掩膜的HSV蓝色空间的上下限
global blue_upper_pau_turn
global hu_pau   # 用于进行Pau和Turn判定的Hu矩
global hu_turn


# 一个初始化全局变量的函数
# 包括了三标牌触发器识别，Pau Tuan形状识别
def init_detect_portable():
    global blue_lower_rls
    global blue_upper_rls
    global kernel_set
    global cap_camera_wig
    global blue_lower_pau_turn
    global blue_upper_pau_turn
    global hu_pau
    global hu_turn

    # 下面是三个圆形标志检测蓝色部分用到的HSV掩膜范围
    blue_lower_rls = np.array([110, 140, 60])
    blue_upper_rls = np.array([129, 250, 255])
    # 生成用于形态学操作的矩阵
    kernel_set = cv2.getStructuringElement(shape=cv2.MORPH_RECT,
                                           ksize=(5, 5),
                                           anchor=(-1, -1)
                                           )

    # -----------------------------------------------------------------------------------
    # 下面是P标志检测的基本设置
    # 接下来就是提取出来蓝色区域的掩码
    # 首先定义交通标志蓝色区域的HSV掩膜范围
    blue_lower_pau_turn = np.array([110, 60, 50])
    blue_upper_pau_turn = np.array([140, 200, 255])

    # 定义交通标志模板白色的(B:G:R)掩膜的范围
    White_lower = np.array([130, 130, 130])
    White_upper = np.array([255, 255, 255])
    # 读取交通标志模板图片
    img_source_match_pau = cv2.imread("/home/nano/config/teleop/src/smartcar/image/traffic_Pause.png")
    img_source_match_turn = cv2.imread("/home/nano/config/teleop/src/smartcar/image/traffic_turn2.png")

    # 进行类似二值化的操作
    white_pau = cv2.inRange(src=img_source_match_pau,
                            lowerb=White_lower,
                            upperb=White_upper)

    white_turn = cv2.inRange(src=img_source_match_turn,
                             lowerb=White_lower,
                             upperb=White_upper)

    # 显示二值化的图片
    # cv2.imshow('pau', white_pau)
    # cv2.imshow('turn', white_turn)

    # 提取轮廓
    contours_pau, hierarchy_pau = cv2.findContours(image=white_pau,
                                                   mode=cv2.RETR_LIST,
                                                   method=cv2.CHAIN_APPROX_NONE
                                                   )
    contours_turn, hierarchy_turn = cv2.findContours(image=white_turn,
                                                     mode=cv2.RETR_LIST,
                                                     method=cv2.CHAIN_APPROX_NONE
                                                     )

    # 得到Hu矩
    hu_pau = cv2.HuMoments(cv2.moments(contours_pau[1]))
    hu_turn = cv2.HuMoments(cv2.moments(contours_turn[0]))


# 摄像头畸变矫正函数
def Straighten_the_camera(original_img):
    # 这里对摄像头的视频帧进行矫正
    # 定义相机内参矩阵，畸变矩阵，用来矫正相机畸变：相机型号HF867
    CamMax = np.array([[367.87792233, 0, 320.00047236],
                       [0, 370.36759206, 240.00000000],
                       [0, 0, 1]])

    disCef = np.array([-0.3369, 0.0870, 0, 0, 0])

    # 这里就是矫正摄像头畸变的操作
    und_st = cv2.undistort(src=original_img,
                           cameraMatrix=CamMax,
                           distCoeffs=disCef,
                           newCameraMatrix=CamMax
                           )
    return und_st


# 找到有效的(内部又足够蓝色点的)圆形掩码的函数
def get_effective_circle_blue_mask(img_search_circle):
    # 读取一些全局变量的值
    global blue_lower_rls
    global blue_upper_rls
    global kernel_set
    global blue_lower_pau_turn
    global blue_upper_pau_turn
    global hu_pau

    # 首先读取图片：这里用的图片是交通标志里面的，后面如果报错，可以更换图片
    img_source_rig = img_search_circle.copy()
    # 给蓝色检测留下备份
    img_judge_blue = img_search_circle.copy()

    # ---------------------------------------------首先是对于圆形掩膜的检测
    # 将图片灰度化
    img_gray_rig = cv2.cvtColor(img_source_rig, code=cv2.COLOR_BGR2GRAY)

    # 下面进行检测圆形的操作
    circles = cv2.HoughCircles(image=img_gray_rig,
                               method=cv2.HOUGH_GRADIENT,
                               dp=1,
                               minDist=50,
                               param1=80,
                               param2=100,
                               minRadius=0,
                               maxRadius=200)

    # 声明绘制圆形的颜色
    circle_color = (153, 0, 130)

    # 这里加一层判断条件，如果检测到了圆形(且半径大于阈值)，就打印圆的数据，并且在图片上进行绘制
    if circles is not None and 40 <= circles[0][0][2] <= 150:
        # print("find the circle")
        # 打印检测到的圆的数据
        # print(circles[0])
        # print(circles[0][0][0], circles[0][0][1], circles[0][0][2])
        # print('radius:', circles[0][0][2])
        # 进行复制操作
        circle_mask_img = img_source_rig.copy()

        # 下面是绘制圆形的操作
        cv2.circle(img=circle_mask_img,
                   # center=(320, 240),
                   center=(int(circles[0][0][0]), int(circles[0][0][1])),
                   radius=int(circles[0][0][2]),
                   color=circle_color,  # 这里使用了全局声明的绘制圆形的颜色
                   thickness=-1,  # 这里绘制实心的圆形
                   lineType=cv2.LINE_AA,
                   shift=0,
                   )
        # 显示绘制圆形的结果
        # cv2.imshow('result_find_circle', circle_mask_img)

        # 下面是创建圆形掩膜的操作
        # 定义交通标志(B:G:R)掩膜的范围
        # 这里是由绘制圆形的颜色，使用定义的范围来检测，这样可以提取出绘制的圆形掩码
        signal_lower = np.array(circle_color)
        signal_upper = np.array(circle_color)

        # 创建提取出来的交通标志圆形的掩膜
        Signal_mask = cv2.inRange(src=circle_mask_img,
                                  lowerb=signal_lower,
                                  upperb=signal_upper)

        # 展示提取到的白色掩膜的图片
        # cv2.imshow('signal_mask', Signal_mask)
        # --------------------------------------------------------------------------------
        # 这里还要进行一步操作，就是检测圆形里面的蓝色点情况
        # 先使用掩膜将需要检测蓝色点的掩码的圆形的部分提取出来
        circle_mask_test_blue = cv2.bitwise_and(src1=img_judge_blue,
                                                src2=img_judge_blue,
                                                mask=Signal_mask)
        # cv2.imshow('cal_mask', circle_mask_test_blue)

        # 进行HSV颜色空间的转换
        img_HSV_circle_mask = cv2.cvtColor(circle_mask_test_blue, cv2.COLOR_BGR2HSV)
        # 将蓝色掩膜提取出来
        blue_mask = cv2.inRange(src=img_HSV_circle_mask,
                                lowerb=blue_lower_rls,
                                upperb=blue_upper_rls)
        # ----------------------------------------------------------这里尝试进行去除噪点的操作，形态学开操作(非必要)
        # 进行开运算操作
        open_cal_get = cv2.morphologyEx(src=blue_mask,
                                        op=cv2.MORPH_OPEN,
                                        kernel=kernel_set,
                                        iterations=1)

        # ----------------------------------------------------------------------------------
        # cv2.imshow('blue_mask_open_cal', open_cal_get)

        # 计算蓝色掩膜当中点的数量
        B_count = cv2.countNonZero(blue_mask)

        # print("the blue_count:", B_count)

        # 计算一下判断值
        radius = int(circles[0][0][2])
        # 这里的计算方法，就是用圆形当中蓝色的点，来除以圆的半径^2
        # 经过测试，离得越远，这个值会越小，/////后面更正之后，就没有这个结论了
        # judge_threshold = B_count / (radius ^ 2)   # ///////这个是你之前写的错误算法，这里更正，，否则结果就是直接除以radius
        judge_threshold = B_count / pow(radius, 2)  # //这个才是正确的写法
        # print('judge_result:', judge_threshold)

        # 进行掩码是否找到的判断，这里设置的阈值是70
        # ////更正之后，经过测试，都是2.几,所以阈值重新设置在2~3之间
        if 2 <= judge_threshold <= 3:
            print('find the effective circle mask')
            flag_traffic = 123
            return open_cal_get, flag_traffic

        else:
            # print('find the circle but not effective')
            None_mask = np.zeros((480, 640, 3), np.uint8)
            flag_traffic = 0
            return None_mask, flag_traffic

    ######################----------------------------------------------------------------------
    # 没有找到圆形，显示没有圆形的结果，黑屏
    # 接下来进行pau标志的检测
    else:
        # print("Can't find circle")
        # 将掩膜设置为空
        None_mask = np.zeros((480, 640, 3), np.uint8)
        flag_traffic = 0
        return None_mask, flag_traffic


# 找到有效的Pau标志
def get_pause_shape(image_search_pau):
    # 读取一些全局变量的值
    global kernel_set
    global blue_lower_pau_turn
    global blue_upper_pau_turn
    global hu_pau
    # 读取到对应的图片
    img_source_rig = image_search_pau
    # 进行颜色空间的转换
    img_HSV_rig = cv2.cvtColor(img_source_rig, cv2.COLOR_BGR2HSV)
    # 创建蓝色的掩膜
    Blue_mask = cv2.inRange(src=img_HSV_rig,
                            lowerb=blue_lower_pau_turn,
                            upperb=blue_upper_pau_turn)
    # ---------------------------------------------------------------------
    # --这里也是进行一个形态学操作，来去除部分的噪点(非必要)
    # 进行开运算操作
    Blue_mask = cv2.morphologyEx(src=Blue_mask,
                                 op=cv2.MORPH_OPEN,
                                 kernel=kernel_set,
                                 iterations=1)
    # ---------------------------------------------------------------------
    B_Count = cv2.countNonZero(Blue_mask)
    # print("B_count:", B_Count)
    # 检查蓝色掩膜点的数量，看是否扫描到了交通标牌
    if B_Count >= 4000:
        # 下面是对于识别图像的操作
        # 查找轮廓
        contours_mask, hierarchy_img = cv2.findContours(image=Blue_mask,
                                                        mode=cv2.RETR_LIST,
                                                        method=cv2.CHAIN_APPROX_NONE
                                                        )
        # 建立一个存储Hu匹配值的列表
        res_pau = []
        for i in range(len(contours_mask)):
            hu1 = cv2.HuMoments(cv2.moments(contours_mask[i]))
            dist_pau = cv2.matchShapes(hu1, hu_pau, cv2.CONTOURS_MATCH_I2, 0)

            # 将匹配得到的结果存储到列表当中,这里排除掉错误的匹配点（不算形状）
            if dist_pau != 0:
                res_pau.append(dist_pau)

        # 取得所有结果的最小值
        result_pau = min(res_pau)

        # //------------------------测试所用--------------------//
        # 找出匹配最优秀的轮廓的索引
        for i in range(len(contours_mask)):
            hu1 = cv2.HuMoments(cv2.moments(contours_mask[i]))
            dist_pau = cv2.matchShapes(hu1, hu_pau, cv2.CONTOURS_MATCH_I2, 0)
            if dist_pau == result_pau:
                contours_index = i
                # print("index:", i)

        # 设置一下轮廓的编号，这里虽然有警告，但是实际上逻辑不会出错的
        m = int(contours_index)
        # 进行轮廓的面积计算
        area_contours_yangben = int(cv2.contourArea(contour=contours_mask[m],
                                                    oriented=False))
        # 这里进行轮廓的周长运算
        length_contours_yangben = int(cv2.arcLength(curve=contours_mask[m],
                                                    closed=True))
        # 进行面积除以周长平方的计算，计算一个用来判断的值
        judge_area_length_yangben = float(area_contours_yangben) / float(pow(length_contours_yangben, 2)) * 1000

        # print('judge_yangben:', judge_area_length_yangben)

        # //------------------------------------------------------//

        # 打印形状匹配的结果
        # print('the result_pau:', result_pau)
        # 这里的筛选条件还用到了周长和面积的限制
        if result_pau <= 1.0 and area_contours_yangben >= 2000 and 30 <= judge_area_length_yangben <= 50:

            # ------------------------------------------------------
            # 这里是将找到的匹配轮廓绘制出来，测试用的
            # 整体来说，还是会被干扰，但是你有了一个用面积来去除干扰的想法
            # 绘制匹配度最高的轮廓
            sour_cp = image_search_pau.copy()
            cv2.drawContours(image=sour_cp,
                             contours=contours_mask,
                             contourIdx=contours_index,
                             color=(0, 0, 255),  # 这里选择了红色的轮廓线
                             thickness=2,
                             lineType=cv2.LINE_AA
                             )
            cv2.imshow('calcul_mask_P', sour_cp)
            # -------------------------------------------------------

            print("Pause")
            flag_traffic = 4
            return Blue_mask, flag_traffic


        else:
            # print("No circle,maybe is P")
            None_mask = np.zeros((480, 640, 3), np.uint8)
            flag_traffic = 0
            # print("None_P")
            return None_mask, flag_traffic

    else:
        # print("No circle,maybe is P")
        None_mask = np.zeros((480, 640, 3), np.uint8)
        flag_traffic = 0
        # print("None_P")
        return None_mask, flag_traffic


# 找到有效的Turn标志
def get_turn_shape(image_search_turn):
    global kernel_set
    global blue_lower_pau_turn
    global blue_upper_pau_turn
    global hu_turn
    # 读取到对应的图片
    img_source_rig = image_search_turn
    # 进行颜色空间的转换
    img_HSV_rig = cv2.cvtColor(img_source_rig, cv2.COLOR_BGR2HSV)
    # 创建蓝色的掩膜
    Blue_mask = cv2.inRange(src=img_HSV_rig,
                            lowerb=blue_lower_pau_turn,
                            upperb=blue_upper_pau_turn)
    # ---------------------------------------------------------------------
    # --这里也是进行一个形态学操作，来去除部分的噪点(非必要)
    # 进行开运算操作
    Blue_mask = cv2.morphologyEx(src=Blue_mask,
                                 op=cv2.MORPH_OPEN,
                                 kernel=kernel_set,
                                 iterations=1)
    # ---------------------------------------------------------------------
    B_Count = cv2.countNonZero(Blue_mask)
    # print("B_count:", B_Count)
    # 检查蓝色掩膜点的数量，看是否扫描到了交通标牌
    if B_Count >= 4000:
        # 下面是对于识别图像的操作
        # 查找轮廓
        contours_mask, hierarchy_img = cv2.findContours(image=Blue_mask,
                                                        mode=cv2.RETR_LIST,
                                                        method=cv2.CHAIN_APPROX_NONE
                                                        )
        # 建立一个存储Hu匹配值的列表
        res_turn = []
        for i in range(len(contours_mask)):
            hu2 = cv2.HuMoments(cv2.moments(contours_mask[i]))
            dist_turn = cv2.matchShapes(hu2, hu_turn, cv2.CONTOURS_MATCH_I2, 0)

            # 将匹配得到的结果存储到列表当中,这里排除掉错误的匹配点（不算形状）
            if dist_turn != 0:
                res_turn.append(dist_turn)

        # 取得所有结果的最小值
        result_turn = min(res_turn)

        # //------------------------测试所用--------------------//
        # 找出匹配最优秀的轮廓的索引
        for i in range(len(contours_mask)):
            hu2 = cv2.HuMoments(cv2.moments(contours_mask[i]))
            dist_turn = cv2.matchShapes(hu2, hu_turn, cv2.CONTOURS_MATCH_I2, 0)
            if dist_turn == result_turn:
                contours_index = i
                # print("index:", i)

        # 设置一下轮廓的编号
        m = int(contours_index)
        # 进行轮廓的面积计算
        area_contours_yangben = int(cv2.contourArea(contour=contours_mask[m],
                                                    oriented=False))
        # 这里进行轮廓的周长运算
        length_contours_yangben = int(cv2.arcLength(curve=contours_mask[m],
                                                    closed=True))
        # 进行面积除以周长平方的计算，计算一个用来判断的值
        judge_area_length_yangben = float(area_contours_yangben) / float(pow(length_contours_yangben, 2)) * 1000

        # print('judge_yangben_turn:', judge_area_length_yangben)

        # //------------------------------------------------------//

        # 打印形状匹配的结果
        # print('the result_turn:', result_turn)
        # 这里的筛选条件还用到了周长和面积的限制,具体的阈值judge_yangben需要手动进行调参
        if result_turn <= 1.0 and area_contours_yangben >= 2000 and 10 <= judge_area_length_yangben <= 20:

            # ------------------------------------------------------
            # 这里是将找到的匹配轮廓绘制出来，测试用的
            # 整体来说，还是会被干扰，但是你有了一个用面积来去除干扰的想法
            # 绘制匹配度最高的轮廓
            sour_cp = image_search_turn.copy()
            cv2.drawContours(image=sour_cp,
                             contours=contours_mask,
                             contourIdx=contours_index,
                             color=(0, 0, 255),  # 这里选择了红色的轮廓线
                             thickness=2,
                             lineType=cv2.LINE_AA
                             )
            cv2.imshow('calcul_mask_T', sour_cp)
            # -------------------------------------------------------

            print("Turn")
            flag_traffic = 5
            return Blue_mask, flag_traffic


        else:
            # print("No circle,maybe is P")
            None_mask = np.zeros((480, 640, 3), np.uint8)
            flag_traffic = 0
            # print("None_T")
            return None_mask, flag_traffic

    else:
        # print("No circle,maybe is P")
        None_mask = np.zeros((480, 640, 3), np.uint8)
        flag_traffic = 0
        # print("None_T")
        return None_mask, flag_traffic


# 用于执行上述标牌判断的函数
def Judge_execute_functions(img_get):
    try:

        # 这里直接获取圆形掩膜，并仅从初步的触发判断
        search_mask_rls, flag_tra_rls = get_effective_circle_blue_mask(img_get)
        search_mask_P, flag_tra_P = get_pause_shape(img_get)
        search_mask_T, flag_tra_T = get_turn_shape(img_get)


        # cv2.imshow('blue_mask_open_cal_rls', search_mask_rls)
        # cv2.imshow('Pause_mask', search_mask_P)
        # cv2.imshow('Turn_mask', search_mask_T)
        # cv2.imshow('camera_image_1', img_get)

        # 这里就是看有没有识别到特定的元素，用于元素表控制
        if flag_tra_rls or flag_tra_P or flag_tra_T:
            return 1
        else:
            return 0
    except:

        # 显示一个红色警告窗口
        # img_get = np.full((480, 640, 3), (0, 0, 255), np.uint8)
        cv2.imshow('error_cam_img_1', img_get)
        print("Execute_functions was wrong!!")
        return 66


if __name__ == '__main__':

    # 初始化所需要的基本的参数
    init_detect_portable()

    # 首先读取摄像头
    # ubuntu下必须要使用cv2.CAP_V4L2这个采集参数，否则会出现1280*720的分辨率
    cap_camera_wig = cv2.VideoCapture(0, cv2.CAP_V4L2)      # cv_ubuntu_cp36

    # 创建好窗口（非必要）
    # cv2.namedWindow('camera_image_1', cv2.WINDOW_NORMAL)
    # cv2.namedWindow('blue_mask_open_cal_rls', cv2.WINDOW_NORMAL)
    # cv2.namedWindow('Pause_mask', cv2.WINDOW_NORMAL)
    # cv2.namedWindow('Turn_mask', cv2.WINDOW_NORMAL)

    while True:
        # 读取摄像头的获取到的视频帧
        ret1, frame1 = cap_camera_wig.read()

        # 如果读取到了视频帧
        if ret1:
            # 进行畸变的矫正
            camera_get = Straighten_the_camera(frame1)

        # 否则报错，并且退出进程
        else:
            print("can't get the pic")
            sys.exit()

        # 进行标志的判断
        Judge_execute_functions(camera_get)

        # 等待10ms看有没有按键按下，读取指令
        key_wig = cv2.waitKey(1)

        # 这里设置了退出的指令
        if (key_wig & 0xFF) == ord('q'):
            print("Exit the programme")
            break

    # 释放VideoCapture这个函数产生的变量 cap_wig
    cap_camera_wig.release()

    # 释放窗口
    cv2.destroyAllWindows()
